#include <linux/sched.h>
#include <linux/sched/task.h>
#include <linux/thread_info.h>
#include <arm/v7m.h>
#include <asm/io.h>
#include <arm/bug.h>

void* __pendsv_do_switch(void *sp)
{
   struct task_struct *prev = current;
    struct thread_info *prev_ti = task_thread_info(prev);
    struct task_struct *next;

#ifdef CONFIG_ARM_DEBUG_PENDSV
    show_regs((struct pt_regs *)sp);
#endif

    prev_ti->cpu_context.sp = sp;

    if (prev_ti->next == NULL) /* triggered from ISR */
    {
        schedule();
    }

    __raw_writel(V7M_SCB_ICSR_PENDSVCLR, BASEADDR_V7M_SCB + V7M_SCB_ICSR);

    next = prev_ti->next;
    if (next != NULL)
    {
        prev_ti->next = NULL;
        set_current(next);
        sp = task_thread_info(next)->cpu_context.sp;
    }

    return sp;
}
